/*
 * Map.cpp
 *
 *  Created on: 20/10/2009
 *      Author: marcos
 */

#include "Map.h"
#include "OverlappingMatcher.h"
#include "RoadNameMatcher.h"
#include "core/StringToIdMatcher.h"
#include "exceptions/InvalidStrategyException.h"
#include "exceptions/OverlappingRoadSegmentException.h"
#include "util/logger/LoggerManager.h"
#include "util/MathUtils.h"
#include "util/StringUtils.h"
#include <math.h>
#include <sstream>

Map::Map() :
timeSolver(new path::TimeCost()), distanceSolver(new path::DistanceCost())
{
}

Map::~Map() {
	delete timeSolver.getEdgeCost();
	delete distanceSolver.getEdgeCost();
}
/* Convierte un path a una ruta */
Route* pathToRoute(path::Path* path);
/* Calcula el promedio ponderado entre ambos valores */
double weightedAverage(double weight, double a, double b);

MapLocation* Map::addLocation(string id, string edgeId, int height,
		MapLocationType type)
throw (DuplicateElementException) {
	StringToIdMatcher<MapLocation*> matcher(id);
	if (this->mapLocations.contains(&matcher))
		throw(DuplicateElementException("El objeto mapa \"" + id  + "\" ya existe."));

	try {
		RoadSegment* segment = this->getSegmentContainingEdge(edgeId);
		double offsetX = 0.0;
		double offsetY = 0.0;
		double rotation = 0.0;
		double metersToIncrese = 12.0; // cte
		// Codigo para el calculo de la ubicacion de los MapLocation dentro del RoadSegment en que se encuentran.
		double x0 = segment->getOrigin()->getX();
		double y0 = segment->getOrigin()->getY();
		double x1 = segment->getDestination()->getX();
		double y1 = segment->getDestination()->getY();
		double percentage = segment->increasingPercentage(height);
		double xPos = x0 + percentage * (x1 - x0);
		double yPos = y0 + percentage * (y1 - y0);

		// calculo si la altura es par o impar
		int rest = height % 2;
		bool par = rest == 1? false : true;

		// TODO : Verificar que estas cuentas realmente resuelvan el problema para todos los casos.
		//angulo = arcotangente (opuesto, adyacente);
		rotation = atan2(y1 - y0, x1 - x0);
		// Si el objeto esta de lado par, le sumo al angulo del camino, pi/2, sino se lo resto
		// De esta forma obtengo el angulo de la recta perpendicular al camino, sobre la que se ubicara el objeto.
		rotation += par? PI/2 : -PI/2;

		// calculo el corrimiento en x e y. adyacente = cos() * hipotenusa; opuesto = sen() * hipotenusa
		offsetX = cos(rotation) * metersToIncrese;
		offsetY = sin(rotation) * metersToIncrese;
		// por ultimo, corro las coordenadas XY del objeto segun corresponda
		xPos += offsetX;
		yPos += offsetY;

		// al angulo de rotacion (el parametro 'rotation'), se le suma PI/2 para desplazar el inicio del angulo al eje Y
		MapLocation* location = new MapLocation(id, segment->getRoad()->getName(), height, type, xPos, yPos, rotation + PI / 2);
		this->mapLocations.add(0, location);
		bool firstTime = this->graph.isEmpty();
		updateHorizontalAxis(xPos, firstTime);
		updateVerticalAxis(yPos, firstTime);
		return location;
	} catch (ElementNotFoundException e) {
		throw(DuplicateElementException("La arista \"" + edgeId  + "\" no existe."));
	}

}

Road* Map::addRoad(string id, string roadName, string origin,
		string destination, double length, double maxSpeed,
		RoadType roadType, int minHeight, int maxHeight)
throw (DuplicateElementException, ElementNotFoundException) {
	Vertex* vOrigin = this->graph.getVertex(origin);
	Vertex* vDestination = this->graph.getVertex(destination);
	Road* road = getOrBuildRoad(roadName, roadType);
	try {
		this->graph.getEdge(id);
		throw(DuplicateElementException("El camino \"" + id  + "\" ya existe."));
	} catch (ElementNotFoundException e) {
		// TODO Analizar si esta aproximaci�n corresponde, ya que podr�a haber
		// un error al crear el Edge lo cual podr�a significar que debe eliminarse el segmento si no tiene otras aristas
		Intersection iOrigin(vOrigin->getPositionX(), vOrigin->getPositionY());
		Intersection iDestination(vDestination->getPositionX(), vDestination->getPositionY());
		RoadSegment* segment = NULL;
		try {
			segment = road->getSegment(minHeight, maxHeight, iOrigin, iDestination);
		} catch (OverlappingRoadSegmentException &e) {
			throw DuplicateElementException("Ya existe la altura "
					+ util::intToString(minHeight) + "-" + util::intToString(
					maxHeight) + " en la calle " + roadName);
		}
		segment->addEdge(this->graph.addEdge(id, vOrigin, vDestination, length, maxSpeed), minHeight < maxHeight);
	}
	return road;
}
Road* Map::getOrBuildRoad(string roadName, RoadType roadType) {
	try {
		RoadNameMatcher matcher(roadName);
		return this->roads.search(&matcher);
	} catch (ElementNotFoundException e) {
		Road* newRoad = new Road(roadName, roadType);
		this->roads.add(0, newRoad);
		return newRoad;
	}
}
Road* Map::getRoad(string roadName) throw(RoadNotFoundException) {
	RoadNameMatcher matcher(roadName);
	try {
		return this->roads.search(&matcher);
	} catch (ElementNotFoundException &e) {
		throw RoadNotFoundException("No se encontro el camino \"" + roadName + "\".");
	}
}
Route* Map::route(string originRoadName, int originHeight,
		string destinationRoadName, int destinationHeight, RouteOptimize optimizationStrategy)
	throw(RoadNotFoundException, OutOfReachException) {
	Road* originRoad = getRoad(originRoadName);
	logger()->info("Found Road " + originRoad->getName());
	Road* destinationRoad = getRoad(destinationRoadName);
	logger()->info("Found Road " + destinationRoad->getName());
	util::LinkedList<RoadSegment*>* originSegments = originRoad->getSegments(originHeight);
	util::LinkedList<RoadSegment*>* destinationSegments = destinationRoad->getSegments(destinationHeight);
	if (originSegments->isEmpty()) throw RoadNotFoundException(originRoadName, originHeight);
	if (destinationSegments->isEmpty()) throw RoadNotFoundException(destinationRoadName, destinationHeight);

	Vertex* originVertex = splitOutgoingEdges(originSegments, originHeight);
	Vertex* destinationVertex = splitIncomingEdges(destinationSegments, destinationHeight);

	return pathToRoute(this->graph.solvePath(*solver(optimizationStrategy), originVertex, destinationVertex));
}

void Map::addIntersection(string id, double position_x, double position_y)
throw (DuplicateElementException) {
	bool firstTime = this->graph.isEmpty();
	this->graph.addVertex(id, position_x, position_y);
	updateHorizontalAxis(position_x, firstTime);
	updateVerticalAxis(position_y, firstTime);
}

path::DijkstraShortestPath* Map::solver(RouteOptimize optimizationStrategy) {
	switch(optimizationStrategy) {
		case Time:
			return &this->timeSolver;
		case Distance:
			return &this->distanceSolver;
		default:
			throw InvalidStrategyException();
	}

}
util::LinkedList<RoadSegment*>* Map::getAllSegments() {
	util::LinkedList<Road*>::Iterator* it = this->roads.iterator();
	util::LinkedList<RoadSegment*>* segments = new util::LinkedList<RoadSegment*>();
	while (it->hasNext()) {
		segments->pushAll(it->next()->getSegments());
	}
	delete it;
	return segments;
}

Vertex* Map::splitOutgoingEdges(util::LinkedList<RoadSegment*>* segments, int height) {
	logger()->info("Splitting Outgoing: ");
	util::LinkedList<RoadSegment*>::Iterator* it = segments->iterator();
	Vertex* originVertex = this->graph.addVertex("origin",0.0,0.0);
	// Si bien se recorre una lista como m�ximo tiene 2 elementos, y es en el caso que se pidio una altura que
	// corresponde a 2 segmentos, en cuyo caso el vertice generado no se utilizar� en el Route final. Por lo cual,
	// para ese caso, no importa que posici�n x,y tenga el vertice
	while(it->hasNext()) {
		RoadSegment* segment = it->next();
		this->splitFrom(originVertex, segment->getIncreasingEdges(), segment->decreasingPercentage(height));
		this->splitFrom(originVertex, segment->getDecreasingEdges(), segment->increasingPercentage(height));
		originVertex->setPositionX(weightedAverage(segment->increasingPercentage(height),
				segment->getOrigin()->getX(), segment->getDestination()->getX()));
		originVertex->setPositionY(weightedAverage(segment->increasingPercentage(height),
				segment->getOrigin()->getY(), segment->getDestination()->getY()));
	}
	delete it;
	return originVertex;
}

Vertex* Map::splitIncomingEdges(util::LinkedList<RoadSegment*>* segments, int height) {
	logger()->info("Splitting Incoming: ");
	util::LinkedList<RoadSegment*>::Iterator* it = segments->iterator();
	Vertex* destinationVertex = this->graph.addVertex("destination",0.0,0.0);
	// Si bien se recorre una lista como m�ximo tiene 2 elementos, y es en el caso que se pidio una altura que
	// corresponde a 2 segmentos, en cuyo caso el vertice generado no se utilizar� en el Route final. Por lo cual,
	// para ese caso, no importa que posici�n x,y tenga el vertice
	while(it->hasNext()) {
		RoadSegment* segment = it->next();
		this->splitTo(destinationVertex, segment->getIncreasingEdges(), segment->increasingPercentage(height));
		this->splitTo(destinationVertex, segment->getDecreasingEdges(), segment->decreasingPercentage(height));
		destinationVertex->setPositionX(weightedAverage(segment->increasingPercentage(height),
				segment->getOrigin()->getX(), segment->getDestination()->getX()));
		destinationVertex->setPositionY(weightedAverage(segment->increasingPercentage(height),
				segment->getOrigin()->getY(), segment->getDestination()->getY()));
	}
	delete it;
	return destinationVertex;
}
void Map::splitFrom(Vertex* adHocVertex, util::LinkedList<Edge*>* edges, double percentage) {
	// Las aristas que requieren partir el 100% no se generan ya que es preferible que se pase
	// por el v�rtice real
	if (percentage < 1.0) {
		stringstream texto;
		string cad;
		texto << "From: " << adHocVertex->getId() << " to " << edges->count() << " vertices, percentage " << percentage;
		logger()->info(texto.str());
		util::LinkedList<Edge*>::Iterator* it = edges->iterator();
		char nombre[256];
		while(it->hasNext()) {
			texto.str("");
			Edge* splitted = it->next();
			sprintf(nombre, "arista aux from %s", splitted->getId().c_str());
			texto.ignore(texto.gcount());

			texto << "Building edge:" << nombre
			<< " (" << adHocVertex->getId() << ") -> (" << splitted->getDestination()->getId() << ") ";

			logger()->info(texto.str());
			this->graph.addEdge(nombre, adHocVertex, splitted->getDestination(),
				splitted->getDistanceInMeters() * percentage, splitted->getSpeedInKMH());
		}
		delete it;
	}
}
void Map::splitTo(Vertex* adHocVertex, util::LinkedList<Edge*>* edges, double percentage) {
	// Las aristas que requieren partir el 100% no se generan ya que es preferible que se pase
	// por el v�rtice real
	if (percentage < 1.0) {
		stringstream texto;
		texto << "To: " << adHocVertex->getId() << " from " << edges->count() << " vertices, percentage " << percentage;
		logger()->info(texto.str());
		util::LinkedList<Edge*>::Iterator* it = edges->iterator();
		char nombre[256];
		while(it->hasNext()) {
			texto.str("");
			Edge* splitted = it->next();
			sprintf(nombre, "arista aux to %s", splitted->getId().c_str());
			texto << "Building edge:" << nombre
			<< " (" << splitted->getOrigin()->getId() << ") -> (" << adHocVertex->getId()  << ") ";
			logger()->info(texto.str());
			this->graph.addEdge(nombre, splitted->getOrigin(), adHocVertex,
					splitted->getDistanceInMeters() * percentage, splitted->getSpeedInKMH());
		}
		delete it;
	}
}

#include "util/Matcher.h"
#include "RoadSegmentMatcherByEdgeId.h"
class SegmentMatcherByEdgeId:
	public Matcher<Road*> {
public:
	SegmentMatcherByEdgeId(string id) { this->id = id; }
	bool matches(Road* road) {
		try {
			RoadSegmentMatcherByEdgeId matcher(this->id);
			roadSegment = road->getSegments()->search(&matcher);
			return true;
		} catch (ElementNotFoundException) {
			return false;
		}
	}
	RoadSegment* getRoadSegment() { return this->roadSegment; }
private:
	string id;
	RoadSegment* roadSegment;
};

RoadSegment* Map::getSegmentContainingEdge(string edgeId) {
	SegmentMatcherByEdgeId matcher(edgeId);
	this->roads.search(&matcher);
	return matcher.getRoadSegment();
}

Route* pathToRoute(path::Path* path) {
	Route* route = new Route();
	// Debido a que los vertices de origen y destino se crearon din�micamente el path tendr�, al menos, 2 edges
	path::Iterator* iterator = path->iterator();
	iterator->next();
	// Verifico si hay que crear la primera interseccion
	if (iterator->transversingEdge()->getDistanceInMeters() > 0.0) {
		route->appendIntersection(new Intersection(iterator->currentVertex()->getPositionX(), iterator->currentVertex()->getPositionY()));
	}
	while (iterator->nextVertex()->getId() != "destination") {
		iterator->next();
		// Recupero todas las intersecciones originales
		route->appendIntersection(new Intersection(iterator->currentVertex()->getPositionX(), iterator->currentVertex()->getPositionY()));
	}

	// Verifico si hay que crear la �ltima interseccion
	if (iterator->transversingEdge()->getDistanceInMeters() > 0.0) {
		route->appendIntersection(new Intersection(iterator->nextVertex()->getPositionX(), iterator->nextVertex()->getPositionY()));
	}
	delete iterator;
	delete path;
	return route;
}

double weightedAverage(double weight, double a, double b) {
	return (b - a) * weight + a;
}

void Map::updateHorizontalAxis(double x, bool firstTime) {
	if (firstTime) {
		this->fartherEast = x;
		this->fartherWest = x;
	} else {
		this->fartherEast = max(this->fartherEast, x);
		this->fartherWest = min(this->fartherWest, x);
	}
}
void Map::updateVerticalAxis(double y, bool firstTime) {
	if (firstTime) {
		this->fartherNorth = y;
		this->fartherSouth = y;
	} else {
		this->fartherSouth = max(this->fartherSouth, y);
		this->fartherNorth = min(this->fartherNorth, y);
	}
}

util::Logger* Map::logger() {
	return util::LoggerManager::getInstance()->getLogger();
}
